clear 
clc
A=[1 0.1;0 2];
%A的行数
n=size(A,1);
B=[0;0.5];
% B的列数
p=size(B,2);
Q=[1 0;0 1];
F=[1 0;0 1];
R=[0.1];
k_steps=100;            %k
X_K=zeros(n,k_steps);   %空的X矩阵
X_K(:,1)=[20;-20];      %x初始值

U_K=zeros(p,k_steps);   %输入矩阵
N=5;                    %预测区间
[E,H]=MPC_Matrices(A,B,Q,R,F,N);
for k = 1:k_steps
U_K(:,k)=Prediction(X_K(:,k),E,H,N,p);
X_K(:,k+1) = A*X_K(:,k)+B*U_K(:,k);
end
%制图
subplot(2,1,1);
hold;
for i = 1:size(X_K,1)
plot(X_K(i,:));
end
legend("x1","x2")
hold off;

subplot(2,1,2);
hold;
for i = 1:size(U_K,1)
plot(U_K(1,:));
end
legend("u1")


%求取代价函数所需的E和H矩阵
function [E,H]=MPC_Matrices(A,B,Q,R,F,N)
n=size(A,1);
p=size(B,2);
M=[eye(n);zeros(N*n,n)];
C=zeros((N+1)*n,N*p);
tmp=eye(n);
%更新M和C
for i=1:N
  rows =i*n+(1:n); %定义当前行数，从i x n开始，共n行
  C(rows,:)=[tmp*B,C(rows-n, 1:end-p)]; %将c矩阵填满
  tmp= A*tmp; %每一次将tmp左乘一次A
  M(rows,:)=tmp; %将M矩阵写满
end
% 定义Q_bar和R_bar
Q_bar = kron(eye(N),Q);% kron 张量积
Q_bar = blkdiag(Q_bar,F);%以Q_bar和F为对角线元素的矩阵
R_bar = kron(eye(N),R); 

%G=M'*Q_bar*M;
E=C'*Q_bar*M;
H=C'*Q_bar*C+R_bar;
end
%二次规划
function u_k= Prediction(x_k,E,H,N,p)
U_k = zeros(N*p,1); % NP x 1
U_k = quadprog(H,E*x_k);
u_k = U_k(1:p,1); % 取第一个结果
end





